#ifndef __DYNAMIC_OBJECT_H
#define __DYNAMIC_OBJECT_H

#include <TooN/TooN.h>

using namespace TooN;
#include <TooN/se3.h>
#include <cvd/image.h>
#include <cvd/byte.h>
#include <boost/logic/tribool.hpp>
#include "ATANCamera.h"
#include "KeyFrame.h"

struct StereoMeasurement
{
  Vector<2> v2ImPos1;
  Vector<2> v2ImPos2;
  Vector<3> v3WorldPos;
};
struct DynObjwithFeatures
{
  std::vector<StereoMeasurement> Features;
  cv::Point2f Center;
  cv::Point2f CenterOld;
  double Theta; //angle of the increment in position
  double incPos; //increment in position
};

struct DynObj
{
  cv::Point3f Center; //position of the center in world frame of reference
  cv::Point2f Size; //estimated size of the object
  cv::Point3f Velocity; //Velocity of the object
  float timeLastObserved;  //Time when the object was last observed
  std::vector<TooN::Vector<4> > lastObservations; //10 last observations

};

class DynamicObject
{
public:
  // Constructor defines size of search patch.
  DynamicObject(const ATANCamera &c, int nPatchSize = 8);

  void NewFrame(uint64_t,boost::shared_ptr<KeyFrame>, boost::shared_ptr<KeyFrame>,  std::vector<TooN::Vector<3> > &vTest);
  std::vector<DynObjwithFeatures> ExtractDynamicObjects(TooN::Matrix<>,std::vector<StereoMeasurement>);
  std::vector<DynObj> ConvertToWorld(std::vector<DynObjwithFeatures>,boost::shared_ptr<KeyFrame>,boost::shared_ptr<KeyFrame>,uint64_t,uint64_t);
  void TrackDynObj(std::vector<DynObj>);


 // void NewFrame2(boost::shared_ptr<KeyFrame>, boost::shared_ptr<KeyFrame>,  std::vector<TooN::Vector<3> > &vTest);
  void NewFrameGrid(boost::shared_ptr<KeyFrame>, std::vector<TooN::Vector<3> > &vTest);
  void CompareOpticalFlow(std::vector<std::vector<cv::Point2f> >);
  void FindCommonMeasurements(boost::shared_ptr<KeyFrame> kf1, boost::shared_ptr<KeyFrame> kf2, std::vector<StereoMeasurement> &meas);
  boost::tribool PotentialDynamicObject(boost::shared_ptr<KeyFrame> kf1, boost::shared_ptr<KeyFrame> kf2, TooN::Vector<2> v2ImPos1, TooN::Vector<3> v3WorldPos);


  TooN::Vector<4> Interpolation(std::vector<StereoMeasurement> &Mlist ,TooN::Vector<2> CenterPx, boost::shared_ptr<KeyFrame> CurrentFrame );
protected:
  ATANCamera mCamera;             // Projection model

  int mnMaxSSD; // This is the max ZMSSD for a valid match. It's set in the constructor.
  int mnPatchSize; // Size of one side of the matching template.
  
  // Some values stored for the coarse template:
  int mnTemplateSum;    // Cached pixel-sum of the coarse template
  int mnTemplateSumSq;  // Cached pixel-squared sum of the coarse template
  inline void MakeTemplateSums(); // Calculate above values
  inline bool Project(const SE3<> &se3CFromW, TooN::Vector<3> v3WorldPos);
  inline double CalcDeterminant(TooN::Matrix<3> M);
  std::vector<std::vector<double> > MatrixObjectFinder(std::vector<std::vector<double> > &M);
  int ZMSSDAtPoint(CVD::BasicImage<CVD::byte> &im, const CVD::ImageRef &ir); // This evaluates the score at one location

  inline Matrix<2> M2Inverse(const Matrix<2> &m)
	    {
    Matrix<2> m2Res;
    double dDet = m[0][0] * m[1][1] - m[1][0] * m[0][1];
    assert(dDet!=0.0);
    double dInverseDet = 1.0 / dDet;
    m2Res[0][0] = m[1][1] * dInverseDet;
    m2Res[1][1] = m[0][0] * dInverseDet;
    m2Res[1][0] = -m[1][0] * dInverseDet;
    m2Res[0][1] = -m[0][1] * dInverseDet;
    return m2Res;
	    };

  Vector<3> mv3Cam;        // Coords in current cam frame
  Vector<2> mv2Image;      // Pixel coords in LEVEL0
  Matrix<2> m2CamDerivs;  // Camera projection derivs
  CVD::Image<CVD::byte> mimTemplate;   // The matching template
  Matrix<2> mm2WarpInverse;   // Warping matrix
  CVD::ImageRef mirCenter;    // Quantized location of the center pixel of the NxN pixel template

  boost::shared_ptr<KeyFrame> OldFrame;
  cv::Mat OldFrameMat;
  std::vector<cv::Point2f> OldFrameFeaturePos;
  uint64_t OldFrameTimestamp;
//  boost::shared_ptr<KeyFrame> OldKeyFrame;
  int nFramesSkiped;
  //TooN::Vector<3> SumTranslation;
  //TooN::SE3<double> SumCurrFromOld;
  std::vector<DynObj> TrackedDynObj;


  TooN::Vector<2> CameraSize;
};

#endif








